COMMENT ⊗   VALID 00009 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	TITLE SSERVO
C00005 00003	 LOOP THROUGH POTS CALCULATE THE DISTANCE TO GO
C00007 00004	 SERVO IN PROGRESS, READ PETIT CLOCK AND CALCULATE DELTA T
C00009 00005	 CHECK IF NEW VELOCITY IS TO BE CALCULATED
C00011 00006	 CALCULATE ELAPSED TIME AND DISTANCE SINCE LAST VELOCITY UPDATE
C00013 00007	CHECK IF DESTINATION AT HAND
C00015 00008	 SET UP IRIS MOTOR (NOT VELOCITY SERVOED)
C00018 00009	 CONSTANTS AND BUFFERS
C00020 ENDMK
C⊗;
TITLE SSERVO
	OPDEF LAC[MOVE]↔OPDEF DAC[MOVEM]↔OPDEF GO[JRST]
	OPDEF DZM[SETZM]↔OPDEF DOM[SETOM]

;REGISTERS AVAILABLE TO USER
	STATE↑:	0   	;STATUS BITS
;POT READINGS - PAN, TILT, FOCUS, ZOOM, IRIS.
	P4↑:0 ↔ P5↑:0 ↔ P6↑:0 ↔ P7↑:0 ↔ P8↑:0 ↔ SREF↑:0		;LATEST.
	L4↑:0 ↔ L5↑:0 ↔	L6↑:0 ↔ L7↑:0 ↔ L8↑:0			;FINAL.

;SW SERVO PROGRAM TO SIERRA TV CAMERA
SCALSTP↑:SPCWAR KILL↔POPJ 17,
SSERVO↑: CONSZ 40↔GO WRONG	;DISMISS IF SPACEWAR  STARTED ON PDP-10
	MOVEI 1,=30↔DAC 1,DELT

;READ THE POTS AND SET UP BYTE POINTERS
	CONI 204,7↔ANDI 7,7	;A/D BUSY?
	SKIPN 7↔GO COUNT
	SKIPE POT↔GO BUSY

COUNT:	MOVEI 6,10		;DATA MISSED COUNT
TURNON:	CONO 204,4250		;SET UP HSDC
	CONO 424,722030		;AUTO INDEX FROM CH. 72
	LAC 5,[-2,,0]
GETIT:	MOVEI 4,200
	CONSO 204,1000
	SOJGE 4,.-1		; WAIT FOR DONE FLAG
	JUMPL 4,HUNG
	CONSZ 204,10000
	GO [	CONO 424,4000	; DATA MISSED, TRY AGAIN
		SOJG 6,[MOVEI 3,12
			SOJGE 3,$.
			GO TURNON]
		GO DMISS]
	DATAI 204,BUFFER(5)
	AOBJN 5,GETIT		;GO BACK FOR SECOND WORD
	CONO 424,4000		;TURN OFF A/D
     	LAC 5,[POINT 12,BUFFER]; SAVE REFERENCE VOLTAGE
	ILDB 10,5
	LSH 10,30
	ASH 10,-30
	FSC 10,233
	DAC 10,SREF
	SKIPE POT
	GO LOP-4		; JUMP AROUND SERVO CODE IF ONLY READING POTS
	LAC 2,[POINT 7,DRVWRD]	;SET UP BYTE POINTERS
	LAC  15,[-5,,0]
; LOOP THROUGH POTS CALCULATE THE DISTANCE TO GO
POINT:
;SUBTRACT DESTINATION FROM BUFFER
	ILDB 10,5↔LSH 10,30↔ASH 10,-30↔DAC 10,HOLD
	LAC 11,L4(15)↔FMPR 11,SREF↔FIX 11,233000
	SUB 10,11
	CAMN 15,[-4,,1]↔DAC 10,TILT  		;SAVE TILT VALUE FOR GRAVITY LOADING
	LAC 14,10				;SAVE THE SIGNED VALUE(TOGO)
	DAC 10,DIFF↔MOVMS 10↔DAC 10,TOGO	;DISTANCE LEFT TO GO
	CAMN 15,[-1,,4]↔GO IRIS			;IRIS CONTROL IS NOT VELOCITY SERVO
     	SKIPE INITAL↔GO INPROG			; SERVO IN PROGRESS, UPDATE
	LAC 13,HOLD↔DAC 13,BEGIN(15)		; FIRST TIME INIT FOR EACH POT
	DAC 10,PREV(15)↔FSC 10,233
     	FDVR  10,[75.0]		;CALCULATE NO. OF SEGMENTS
      	FMPR  10,[66.0]		;NUMBER OF MSECS REQUIRED(66 MSECS/SEGMENT)
        FADR  10,[1000.0]	;ADD IN BASE TIME(1000 MSECS)
        DAC 10,TRAJ		;TOTAL TIME REQUIRED
;AVERAGE VELOCITY REQUIRED(SIGNED)
	LAC 10,14↔FSC 10,233
	FDVR 10,TRAJ↔DAC 10,VELOC(15)↔SETZ 10,
;ABSOLUTE DISTANCE LESS THAN TOLERANCE
	LAC 11,14↔MOVMS 11
	CAIG 11,6↔GO ZERO

	DZM SIGN(15)↔MOVEI 10,20 	;TURN ON MOTORS      
	SKIPG 14↔GO .+3
	MOVEI 10,160↔DOM SIGN(15)

ZERO:	DAC 10,SPEED(15)
	CONI 730,OLDTIM		;STORE START TIME
	GO STPTST 		; CHECK STOP CONDITIONS
; SERVO IN PROGRESS, READ PETIT CLOCK AND CALCULATE DELTA T

INPROG: CONI 730,TIME
	LAC 4,TIME↔LSH 4,-24	;SHIFT OUT MICROSECONDS.
	LAC 11,OLDTIM↔LSH 11,-24
	DZM 13
	CAMN 4,11↔GO TIMEOK	;SKIP IF ANY CHANGE IN SECS AND MINUTES
	LAC 4,TIME↔LSH 4,-32	;CALCULATE TOTAL ELAPSED TIME
	LAC 11,OLDTIM↔LSH 11,-32
	CAME 4,11↔ADD 13,[344703400] 	;ADD IN MINUTE COUNT IN MICROSECS 
	LAC 7,TIME↔LSH 7,12↔LSH 7,-36
	LAC 11,OLDTIM↔LSH 11,12↔LSH 11,-36
	SUB 7,11↔IMUL 7,[3641100]
	ADD 13,7

TIMEOK:	LAC 4,TIME↔LSH 4,=16↔LSH 4,-=16
	LAC 11,OLDTIM↔LSH 11,=16↔LSH 11,-=16
	SUB 4,11			;TOTAL ELAPSED TIME
	ADD 13,4↔FSC 13,233		;FLOAT TIME
	LAC 4,13↔FDVR 4,[1000.0]	;CHANGE TO MSECS

;CHECK TIMEOUT LOOP AND VELOCITY UPDATE 
	LAC 3,4↔FIX 3,233000↔DAC 3,TOTEL	;TOTAL ELAPASED TIME IN MSECS
;TEST IF SPACEWAR MODULE HAS RUN TOO LONG - DISMISS AFTER 9 SECS
	CAIG 3,=9000↔GO TIMCHK
	LAC 7,[1,,0]↔IORM 7,STATE		;NON-TERMINATION OF SSERVO
	GO TRNOFF 
; CHECK IF NEW VELOCITY IS TO BE CALCULATED

TIMCHK:	LAC 14,TOTEL↔SUB 14,DEL1
	CAIG 14,=30↔GO CHECK	;ELAPSED TIME > 30 MSECS
	FSC 14,233↔DAC 14,DELT	;FLOATING ELAPSED TIME SINCE LAST VEL UPDATE
	LAC 14,TOTEL↔DAC 14,DEL1
 	DOM VELCAL+0↔DOM VELCAL+1
	DOM VELCAL+2↔DOM VELCAL+3

;CALCULATE DISTANCE COVERED IF VELOCITY UPDATE
CHECK:	SKIPN VELCAL(15)↔GO OK
       	LAC 6,BEGIN(15)↔LAC 10,HOLD
	SUB 6,10↔FSC 6,233↔DAC 6,DISTNZ(15)
	LAC 14,HOLD↔DAC 14,BEGIN(15)
OK:	LAC 6,TOGO↔CAIL 6,=320↔GO MUMBLE  ;CALCULATE IF DISTANCE TO TARGET < 300
	SKIPE FINAL(15)↔GO DECREM
	DOM FINAL(15)
	LAC 7,DIFF↔DAC 7,STOP(15)
	LAC 7,VELOC(15)↔FDVR 7,[4.0]↔DAC 7,VELSTR(15)
	LAC 7,SPEED(15)↔LAC 7,INDEX
DECREM:	LAC 6,VELSTR(15)	; DECREMENT VELOCITY IF FINAL TURNED ON
        DZM 13↔LAC 4,TOGO
	AOS 13↔SUBI 4,=80
        SKIPLE 4↔GO .-3
        FSC 13,233↔FMPR 6,13
;CHECK FOR SIGN DIFFERENCE 
        SKIPG DIFF↔GO LESS
	SKIPG STOP(15)↔GO LESS+1
	GO POS
; CALCULATE ELAPSED TIME AND DISTANCE SINCE LAST VELOCITY UPDATE

LESS:	SKIPL STOP(15)↔MOVN 6,6
POS:	DAC 6,VELOC(15)
MUMBLE:	SKIPN VELCAL(15)↔GO NEXT
	LAC 11,DISTNZ(15)
	SKIPG INDEX↔LAC 10,BEGIN(15)
        CAMN 10,HOLD↔GO ADDTIM
	SUB 10,HOLD↔FSC 10,233
	FADR 11,10		;ADDITIONAL DISTANCE COVERED
ADDTIM:	SKIPG INDEX↔LAC 7,DELT		;TIME INTERVAL
       	LAC 10,TOTEL
	CAMN 10,DEL1↔GO DOIT
	FSC 10,233
	LAC 13,DEL1↔FSC 13,233
        FSBR 10,13
	FADR 7,10		;ADD IN ADDITIONAL TIME
DOIT: 	SKIPG INDEX

; DO FEEDBACK CALCULATIONS

     	FDVR 11,7       	;AVERAGE VELOCITY OVER PAST 20 MSECS
SKIPG INDEX↔FSBR 11,VELOC(15)	;DIFFERENCE AVERAGE VELOCITY AND PLANNED
				;	VELOCITY(POT UNITS/TIC)
        FDVR 11,VELOC(15)  	;PERCENTAGE DIFFERENCE(SIGNED)
        FMPR  11,GAIN(15)  	;MULTIPLY BY FEEDBACK GAIN
        FIX 11,233000		;FIX VELOCITY GAIN
    	LAC 10,SPEED(15)
	TRNN 10,100↔GO AHEAD	;ADD IN VELOCITY GAIN
	SKIPG DIFF↔MOVNS 11
        SUB 10,11
	CAIG 10,100↔MOVEI 10,100
	CAIL 10,177↔SUBI 10,177
	GO SET
;CHECK IF DESTINATION AT HAND

AHEAD:	SKIPL DIFF↔MOVNS 11
      	ADD 10,11
      	CAIL 10,77↔MOVEI 10,77
	CAIG 10,↔ADDI 10,177
SET:	CAIN 10,↔MOVEI 10,1
;GRAVITY LOADING FOR TILT AND FOCUS
	CAIG 15,2↔CAIN 15, ↔GO NOGRAV
	LAC 7,[0,,3777]
        SUB 7,TILT
	FSC 7,233
	FDVR 7,[4096.0]	;DIVIDE BY 4096
	LAC 6,7↔FMPR 6,6↔FMPR 6,7
	FMPR 7,[0.5706]↔FMPR 6,[-0.64322]↔FADR 6,7	;CALCULATE SIN(X)
	FMPR 6,GRVTRM(15)↔FIX 6,23300
	SKIPL DIFF↔GO DOWNER
	ADD 10,6↔CAIL 10,77↔MOVEI 10,77
	GO NOGRAV

DOWNER:	ADD 10,6↔CAIL 10,177↔MOVEI 10,177
NOGRAV:	DAC 10,SPEED(15)
STPTST:	LAC 7,TOGO		; TEST STOP CONDITIONS
	CAML 7,DEDBND(15)↔GO NEXT
	DZM SPEED(15)
	DOM BEEND(15)
NEXT:	LAC 1,SPEED(15)		;SET UP DRIVE WORD FOR THIS MOTOR
     	IDPB 1,2		;LOAD PROPER VOLTAGE INTO DRIVE WORD
	LAC 13,HOLD
	DAC 13,LAST(15)
	DZM VELCAL(15)
	GO LOOP
; SET UP IRIS MOTOR (NOT VELOCITY SERVOED)

;TOLERANCE ON IRIS - SPECIAL CASE
IRIS:	CAIL 10,20↔GO .+3 
	DOM BEEND(15)↔GO LOOP
	SKIPE INITAL↔GO[
		MOVEI 11,(10)↔SUB 11,PREV(15)↔MOVMS 11
		CAILE 11,3↔GO .+1↔GO LOOP]	;IRIS IS HUNG
	DAC 10,PREV(15)
      	LAC 11,DRVWRD↔TRO 11,100	;SET ENABLE BIT (BIT 29)
        SKIPL HOLD↔TRO 11,200		;SET CLOSE BIT(BIT 28)
        DAC 11,DRVWRD
LOOP:   AOBJN 15,POINT		;BRANCH BACK FOR NEXT FUNCTION
	DOM INITAL
	DATAO 410,DRVWRD	; END OF SERVO IF MOTORS ALL OFF
       	SKIPN DRVWRD↔GO TRNOFF
	DZM DRVWRD↔GO COUNT	; RETURN TO READ POTS AGAIN
; END OF SERVO - CHECK FOR HUNG MOTORS

TRNOFF:	DZM 1 
	SKIPN BEEND+0↔IORI 1,4000
	SKIPN BEEND+1↔IORI 1,2000
	SKIPN BEEND+2↔IORI 1,1000
	SKIPN BEEND+3↔IORI 1,10000
	SKIPN BEEND+4↔IORI 1,100000↔IORM 1,STATE
; ENTER HERE IF ONLY READING POTS
       	LAC 13,[NULL,,NULL+1]↔BLT 13,FINAL+4
	SETZ 15,↔LAC 13,[POINT 12,BUFFER,11]
;DEPOSIT FINAL POT READINGS IN P4-P8
LOP:	ILDB 14,13↔LSH 14,30↔ASH 14,-30
	FSC 14,233↔FDVR 14,SREF↔DAC 14,P4(15)
	AOS 15↔CAIE 15,5↔GO LOP
	MOVEI 1,1↔IORM 1,STATE↔DZM POT		;POTS READ ONLY.
KILL:	DATAO 410,[0]↔DZM INITAL
	DISMIS

;ERRORS...

BUSY:	MOVEI 7,100↔GO DMISS+1
HUNG:	CONO 424,4000↔SKIPA 7,[200]
DMISS:	MOVEI 7,40↔IORM 7,STATE↔GO TRNOFF
WRONG:	MOVEI 7,400↔GO DMISS+1
; CONSTANTS AND BUFFERS

LAST:	BLOCK 5		;LAST POT READING
VELOC:	BLOCK 5		;AVERAGE VELOCITY NEEDED
SPEED:	BLOCK 5		;CURRENT SPEED
BEGIN:	BLOCK 5		;INITIAL POT READING
STOP:	BLOCK 5 
VELSTR: BLOCK 5
PREV:	BLOCK 5		;INITIAL DISTANCE TO GO
VELCAL:	BLOCK 5
DISTNZ:	BLOCK 5
SIGN:	BLOCK 5		;DIRECTION OF MOTOR DACENT
GAIN:	576246,,314632	;-1.35
	576363,,146315	;-1.05
	576400,,0      	;-1.0
	576363,,146315	;-1.05
	576400,,0	;-1.0(NOT USED)
DEDBND: 6		;DRIFT DISTANCE
	=24
	6
	6
	6		;(NOT USED)
GRVTRM: 0		;NOT USED
        203500,,0 	;GRAVITY LOADING TERM FOR TILT(5.0)
	576400,,0	;FOCUS(1.0)
	0		;NOT USED
	0		;NOT USED
S4:	BLOCK 5
INDEX:	0
TOTEL:	0

TILT:	0
DELT:	0
TIME:	0
BUFFER:	BLOCK 2		;RAW POT READINGS
OLDTIM:	0		;LAST TIME READ
NULL:   0		;THIS STARTS THE NULL OUT REGION
DRVWRD:	0
HOLD:	0
TOGO:	0
TRAJ:	0
DIFF:	0
FIRST:	0
DEL1:	0
POT↑:    0
INITAL:	0
BEEND:	BLOCK 5
FINAL:  BLOCK 5

END